%% Nonlinear Models
%  adq@XJTU, 2021-06

%% 齐纳压控电容引发 THD
% RC 低通滤波构型, 但电容 C = C(U) 是 U 的函数, 
% (RC+RU*dC/dU)*dU/dt + U = Uin -> U' = (Uin-U)/(RC+RUC'(U))
% C(U) 通常需要拟合得到解析的表达式; 也可以从数据插值得到插值的值和微分

% a = 4.22e-10;
% b = -0.4683;
% C = @(U) min([a.*power(abs(U),b); 1e-9]);
% dCdU = @(U) min([a*b*power(abs(U),b-1); 2]);

% C0 = 400e-6; kCU = 1e-18;
% C = @(U) C0 + abs(U)*kCU;
% dCdU = @(U) sign(U)*kCU;

C0 = 400e-12; kCU = 1e-9;
C = @(U) C0 + kCU*cosh(U);
dCdU = @(U) kCU*sinh(U);

R = 1e3;
f = 500e3;
A = 0.1;
Uin = @(t) A*sin(2*pi*f*t);

% (RC+RU*dC/dU)*dU/dt + U = Uin -> U' = (Uin-U)/(RC+RUC'(U))
modelFcn = @(t, U) (Uin(t)-U)/(R*C(U)+R*U*dCdU(U));
% modelFcn = @(t, U) (Uin(t)-U)/(R*C(U));

tSpan = [0 20e-6]; U0 = 0;

odeOptions = odeset('Refine', 40, 'AbsTol', 1e-10, 'RelTol', 1e-10);
[t, U] = ode23(modelFcn, tSpan, U0, odeOptions);

% 后处理
Fs = 100*f;
[U_, t_] = resample(U, t, Fs, 'linear');

% figure;
% subplot(211); plot(t, U); title('Original Data');
% subplot(212); plot(t_, U_); title('Resampled Data');

figure;
thd(U_, Fs)

%% 齐纳THD, 扫描参数

Amplitudes = logspace(-3, 1, 13); % 1e-3 到 10 幅度扫描
Frequencies = logspace(3, 6, 10); % 1k-1M 扫描

C0 = 400e-12;
kCU = 1e-9;
C = @(U) C0 + kCU*cosh(U);
dCdU = @(U) kCU*sinh(U);

R = 1e3; % f0 = 400kHz

cyclePerSample = 5;

sinadResult = zeros(length(Amplitudes), length(Frequencies));

for ampIdx = 1:length(Amplitudes)
	A = Amplitudes(ampIdx);	
	for freqIdx = 1:length(Frequencies)
		f = Frequencies(freqIdx);
		Uin = @(t) A*sin(2*pi*f*t);
		modelFcn = @(t, U) (Uin(t)-U)/(R*C(U)+R*U*dCdU(U));
		
		tMax = cyclePerSample / f;
		tSpan = [0 tMax];
		U0 = 0;

		odeOptions = odeset('Refine', 8, 'AbsTol', 1e-6, 'RelTol', 1e-8);
		[t, U] = ode23(modelFcn, tSpan, U0, odeOptions);

		Fs = 50*f;
		U = resample(U, t, Fs, 'linear');
		
		sinadResult(ampIdx, freqIdx) = sinad(U, Fs);
	end
end

[amp, freq] = meshgrid(Amplitudes, Frequencies);
f = figure; ax = axes(f);
surfc(ax, amp, freq, sinadResult');
colorbar;
ax.XScale = 'log'; ax.XLabel.String = 'Amplitude /V'; ax.XDir = 'reverse';
ax.YScale = 'log'; ax.YLabel.String = 'Frequency /Hz'; ax.YDir = 'reverse';
ax.ZLabel.String = 'SINAD /dB';

%% sin thd

% 结论: 20x 过采样, 5x 周期即可

% fsMultiplier = [2 3 5 7 10 20 30 50 70 100];
fsMultiplier = [5 7 10 20 30 50];
cycleCount = 1:.05:6;

thdResult = zeros(length(fsMultiplier), length(cycleCount));
snrResult = zeros(length(fsMultiplier), length(cycleCount));
sinadResult = zeros(length(fsMultiplier), length(cycleCount));

f = 1;

for fsIdx = 1:length(fsMultiplier)
	Fs = fsMultiplier(fsIdx);
	display(fsIdx/length(fsMultiplier));
	for cycleIdx = 1:length(cycleCount)
		N = cycleCount(cycleIdx);
		t = 0:1/Fs:N/f;
		y_hysteretic = sin(2*pi*f*t);
% 		thdResult(fsIdx, cycleIdx) = thd(y, Fs);
% 		snrResult(fsIdx, cycleIdx) = snr(y, Fs);
		sinadResult(fsIdx, cycleIdx) = sinad(y_hysteretic, Fs);
	end
end

% f = 1;
% Fs = 100*f;
% N = 8.5;
% t = 0:1/Fs:N/f;
% 
% y = sin(2*pi*f*t) + randn(size(t));

% figure; sinad(y, Fs);

[Fs, cy] = meshgrid(fsMultiplier, cycleCount);
f = figure; ax = axes(f);
surfc(ax, Fs, cy, sinadResult');
colorbar;
ax.XScale = 'log'; ax.XLabel.String = 'Oversampling ratio'; ax.XDir = 'reverse';
ax.YScale = 'linear'; ax.YLabel.String = 'Timespan /cycles'; ax.YDir = 'reverse';
ax.ZLabel.String = 'SINAD /dB';

%% R, C 均存在非线性
% U'= ( Uin(t) - U ) / ( C(U) + UC'(U) )R(t, i)
% i	= ( UC'(U) + C(U) )U'
% 有 dU/dt = f(t, U, i), i = g(U, dU/dt)
% 
% 由于 R(t, i) 不一定是线性的, 就一般情况而言无法反解为 dU/dt = h(t, U) 的标准形式;
% 且 U' 和 i 存在耦合, 故对最一半的情况手动编写前向欧拉法并用预估-校正方法迭代
% 
% 对最简单的情形, 电阻的热过程时间常数如果远小于输入信号的时间常数, 阻值变化由电流加热造成: 
% R = R0 + a*dt, dt = k*P, P = i2R
% 有 R = R0/(1-a*k*i^2).

%% 考虑电介质弛豫的电容模型
% 对于 n 个 R C 构成的梯形电路, 节点 0, 1, ..., i, ..., n 分别具有 V_i(t) 的电压.
% 令 i = 1 ~ n 为电容系统状态变量, V_0 为系统输入(存在有 ESR 因而可以令输入为电压). 下面有状态方程矩阵:
% 
% $\dot{V}_k = \frac{1}{R_k C_k} V_{k-1} - \frac{1}{C_k} (\frac{1}{R_k} + \frac{1}{R_{k+1}}) V_k + \frac{1}{R_{k+1} C_k} V_{k+1}$
% 边界 1: $\dot{V}_1 = \frac{1}{R_1 C_1} V_0 - \frac{1}{C_1} (\frac{1}{R_1} + \frac{1}{R_2}) V_1 + \frac{1}{R_{2} C_1} V_{2}$
% 边界 2: $\dot{V}_n = \frac{1}{R_n C_n} V_{n-1} - \frac{1}{R_n C_n} V_n$
% 
% 可见为三对角方程. 可以写成 y' = f(t, y) 的形式. y' = A * y + B * u

Rs = [020e-3 001e+9 200e+6 040e+6 005e+6];
Cs = [001e-6 006e-9 003e-9 1.5e-9 001e-9];
% Robert A Pease, Understand capacitor soakage to optimize analog systems, National Semiconductor Corp, October 1982, EDN.com

% Rs = [1 .5 .2];
% Cs = [1 2 3];
n = length(Rs);
diag_up = 1 ./ (Cs(1:end-1) .* Rs(2:end)); % 上对角有 n-1 元素, RC 错开乘, R2C1 开始
diag_down = 1 ./ (Cs(2:end) .* Rs(2:end)); % 下对角有 n-1 元素, RC 对齐乘, R2C2 开始
Rs = [Rs +inf];
diag_main = -1 * (1./Rs(1:end-1) + 1./Rs(2:end)) ./ Cs; % 主对角有 n 元素, RC 对齐+错开
% R1C1+R2C1, ... Rn-1Cn-1+RnCn-1, RnCn; 最后一个由于没有右侧支路, 缺项, 用 0 = 1/inf 补

A = diag(diag_main, 0) + diag(diag_up, +1) + diag(diag_down, -1); % 三对角矩阵构建
B = zeros(n, 1); B(1) = 1;
% 状态变量应当为列向量. cap_fcn 返回一个同维度的列向量, 是状态的微分. 必须全矩阵操作

t1 = 002e-6; % 充电截止
t2 = 004e-6; % 短路截止
t3 = 030e-6; % 开路截止

odeOptions = odeset('Refine', 1, 'AbsTol', 100e-9, 'RelTol', 100e-9);

% 充电
Vext = 001e+0;
cap_fcn = @(~, V) A * V + Vext / (Rs(1) * Cs(1)) * B;
V_init = zeros(n, 1);
[t1s, V1s] = ode23(cap_fcn, [0 t1], V_init, odeOptions);

% 短路
Vext = 000e+0;
cap_fcn = @(~, V) A * V + Vext / (Rs(1) * Cs(1)) * B;
V_init = V1s(end, :);
[t2s, V2s] = ode23(cap_fcn, [0 t2-t1], V_init, odeOptions);

% 开路
cap_fcn = @(~, V) A * V + V(1) / (Rs(1) * Cs(1)) * B;
V_init = V2s(end, :);
[t3s, V3s] = ode23(cap_fcn, [0 t3-t2], V_init, odeOptions);

% 拼接, 绘图
ts = [t1s; t1+t2s; t2+t3s];
Vs = [V1s; V2s; V3s];
sys_energy = sum(.5*Cs.*power(Vs, 2), 2);

f = figure;
hold on
ax = gca;
% ax.XScale = 'log';
grid on

yyaxis left
% ax.YScale = 'linear';
ax.YScale = 'log';
ax.YLim = [1e-6 2];
plot(ts, Vs(:,1), 'DisplayName', '1st node V', 'LineWidth', 2);
plot(ts, Vs(:,2), 'DisplayName', '2st node V', 'LineWidth', 2);
plot(ts, Vs(:,3), 'DisplayName', '3st node V', 'LineWidth', 2);	
ylabel('V');

yyaxis right
ax.YScale = 'log';
plot(ts, sys_energy, 'DisplayName', 'System Energy');
ylabel('J');

xlabel('s');
legend('Location', 'northeast' ,'FontSize', 15);
hold off

%% Backlash Nonlinearity
% 参考 nonlinear_backlash_hyst 的注释

global delta
delta = .8;

% % Test Data 1
% x = [1 2 3 4 4 3 2 1 0 -1 -1 0 1 2 3 4];

% % Test Data 2
% T = 20*(1/50);
% Fs = 5500;
% t = .1:1/Fs:T-1/Fs;
% y_linear = 1 + 5 .* sin(2*pi*50*t) .* exp(-t/.2);

% % Test Data 3
T = 20*(1/50);
Fs = 5500;
t = .1:1/Fs:T-1/Fs;
y_linear = 1 + 5 .* sin(2*pi*50*t) .* exp(-t/.2) ...
	+ 2 .* sin(2*pi*150*t) .* exp(-t/.2);

% % Test Data 4
% x = randi([0 5], [1, 20]);

n= length(y_linear);
y_hysteretic = zeros(1, n);

cstate = 'HI'; xlatch = 0; xprev = 0;
x_init = {cstate, xlatch, xprev};
nonlinear_backlash_hyst(x_init);

for i = 1:n
	y_hysteretic(i) = nonlinear_backlash(y_linear(i));
end


f = figure;

ax1 = nexttile;
hold on;
plot(y_linear, 'DisplayName', 'Input', 'LineWidth', 1);
plot(y_hysteretic, 'DisplayName', 'Output', 'LineWidth', 2);
plot(y_linear + delta/2, 'k:', 'DisplayName', 'Down Ramp', 'LineWidth', .5);
plot(y_linear - delta/2, 'k:', 'DisplayName', 'Up Ramp', 'LineWidth', .5);
legend
xlabel('Tick');
title('Time Domain');

ax2 = nexttile;
hold on;
% axis equal;
plot(y_linear, y_hysteretic);
title('Phase Portrait');
hold off;

%% Backlash Nonlinearity 引发 THD

global delta
delta = 0.005;

T = 1;
Fs = 5e3;
t = .1:1/Fs:T-1/Fs;

y_linear = sin(2*pi*50*t) + .5 .* sin(2*pi*150*t);

n= length(y_linear);

y_hysteretic = zeros(1, n);

cstate = 'HI'; xlatch = 0; xprev = 0;
x_init = {cstate, xlatch, xprev};
nonlinear_backlash_hyst(x_init);

for i = 1:n
	y_hysteretic(i) = nonlinear_backlash(y_linear(i));
end

y_clip = y_linear;
y_clip( y_clip > (+1-delta) ) = +1-delta;
y_clip( y_clip < (-1+delta) ) = -1+delta;

fprintf('SINAD\nLinear:\t%f\nHyst:\t%f\nClip:\t%f\n\n', sinad(y_linear), sinad(y_hysteretic), sinad(y_clip));
fprintf('SFDR\nLinear:\t%f\nHyst:\t%f\nClip:\t%f\n\n', sfdr(y_linear), sfdr(y_hysteretic), sfdr(y_clip));

figure;
nexttile
pspectrum([y_linear], Fs, 'power');
legend({'Linear'});
nexttile
pspectrum([y_hysteretic; y_clip]', Fs, 'power');
legend({'Hysteretic', 'Clipped'});
